/*
 * qsDrivetrain driver for QuickSilver Motor Controllers ported from 
 * Kevin Barry's Player driver to ROS
 * Copyright (C) 2012, Cheng-Lung Lee , University of Detroit Mercy, AMRL lab
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the names of Stanford University or Willow Garage, Inc. nor
 *     the names of its contributors may be used to endorse or promote 
 *     products derived from this software without specific prior written 
 *     permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/*
to test 
   $ rosrun qsDrivetrain qsDrivetrain _UpdateRate:=20








*/

// %Tag(FULLTEXT)%
// %Tag(ROS_HEADER)%

#include "ros/ros.h"

// signal handling
#include <signal.h>
#include <ros/xmlrpc_manager.h>


// use thread for ros::spin() 
#include <boost/thread.hpp>
//#include <boost/lexical_cast.hpp>

// %EndTag(ROS_HEADER)%
// %Tag(MSG_HEADER)%
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float64.h>
#include "std_msgs/String.h"
// %EndTag(MSG_HEADER)%

#include <sstream>
#include <qsDrivetrain/silverlode_commands.h>
#include <qsDrivetrain/silverlode_registers.h>

// use LibSerial, need instal libserial in ubuntu 
// but the libserial v0.6 has bugs when run 115200 BPS
// so ou have to use the svn version or get updated libserial
// http://code.google.com/p/amrl-ros-pkg/downloads/detail?name=libserial-0.6.1.tar.gz
// 
// add CMakeLists.txt 
//      target_link_libraries(qsDrivetrain  serial )
#include <SerialStream.h>
#include <math.h>

// Variables here
// these are for cmd_vel data
double cmd_vel_vx = 0.0;
double cmd_vel_vy = 0.0;
double cmd_vel_va = 0.0;
double cmd_state  = 0;
double cmd_UpdateRate  = 10;

    double sigma_x_;
    double sigma_y_;
    double sigma_theta_;

std::string odom_frame_id;
std::string tf_prefix_;
// these are for 
//tf::TransformBroadcaster tf_;
//ros::Publisher odom_pub_;
//ros::Subscriber cmd_vel_sub_;
using namespace std;
using namespace LibSerial;



void populateCovariance(nav_msgs::Odometry &msg);
void cmdVelReceived(const geometry_msgs::Twist::ConstPtr& cmd_vel);
void chatterCallback(const std_msgs::String::ConstPtr& msg);
void spinThread();
int CheckSerialPort(SerialStream* serial_port,std::string* port,int serial_port_baud,int stop_bit);




// port from Keven's qsDrivetrain.cc
    int GetTics(int32_t &, int32_t &, double &);
    int UpdateOdom(double pyaw = NAN);
    int SetSpeed(double trans_vel, double yaw_rad_per_sec);
    int PetWatchDog();


    double TicsToMeters(int tics, double wc, double tpr);
    int32_t MPSToSVU(double, double);
    /* SilverLode Velocity Units. See the SilverLode user manual */

    int qsQuery(char *, int ,  char *, char *, int, char *, int);
    inline int CheckResponseCode(int, char *);
inline double wrapTo2PI(double theta);
inline double wrapToPI(double theta);
inline void Port_flush(SerialStream* serial_port );
int ReadLine(SerialStream *serial_port,char * output_buff, int  maxlen, int timout );




double    wheelbase              ;
double    wheeldiam              ;
double    wheelcircumference;
double    max_trans_vel          ;
double    max_bias_vel           ;
int       watchdog_hardware_time ;
int       watchdog_software_time ;
int       tics_per_rev           ;
double    motor_gearbox_ratio    ;
double    motor_max_rpm          ;
int       left_id                ;
int       right_id               ;
int port_baud ;
    std::string port_left= "/dev/ttyUSB0";
    std::string port_right= "/dev/ttyUSB1";
    SerialStream serial_port_left;
    SerialStream serial_port_right;

    int32_t LeftTics, RightTics, ltics_last, rtics_last;
    double tics_time_last;
    double last_command_time=0;
	double time_cur,time_last_watchdog;
    int new_command_flag=0;

  double POS_x = 0.0;
  double POS_y = 0.0;
  double POS_theta = 0.0;

  double POS_vx = 0.0;
  double POS_vy = 0.0;
  double POS_va = 0.0;












/**
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char **argv)
{
// %Tag(INIT)%
  ros::init(argc, argv, "qsDrivetrain_ros");



// %EndTag(INIT)%

        ros::NodeHandle node_;

        ros::NodeHandle nh("~");
        nh.param("odometry_frame_id", odom_frame_id, string("odom"));
        nh.param<double>("x_stddev", sigma_x_, 0.006);
        nh.param<double>("y_stddev", sigma_y_, 0.006);
        nh.param<double>("rotation_stddev", sigma_theta_, 0.051);
        nh.param<double>("UpdateRate", cmd_UpdateRate, 10);




       nh.param<double>("wheelbase"             ,wheelbase              , .685          );
       nh.param<double>("wheeldiam"             ,wheeldiam              , .415          );
       nh.param<double>("max_trans_vel"         ,max_trans_vel          , 1             );
       nh.param<double>("max_bias_vel"          ,max_bias_vel           , 1             );
       wheelcircumference = wheeldiam * M_PI;
       nh.param<int>(   "watchdog_hardware_time",watchdog_hardware_time , 300           );
       nh.param<int>(   "watchdog_software_time",watchdog_software_time , 2000          );
       nh.param<int>(   "tics_per_rev"          ,tics_per_rev           , 4000          );
       nh.param<double>("motor_gearbox_ratio"   ,motor_gearbox_ratio    , 12            );
       nh.param<double>("motor_max_rpm"         ,motor_max_rpm          , 4000          );
       nh.param<int>("qs_left_id"            ,left_id                , 1             );
       nh.param<int>("qs_right_id"           ,right_id               , 2             );

       nh.getParam("port_left"             ,port_left       );
       nh.getParam("port_right"            ,port_right     );
       nh.param<int>("port_baud"             ,port_baud       , 115200        );


        tf_prefix_ = tf::getPrefixParam(node_);
    std::stringstream ss1;
    ss1 << "P UpdateRate: " << cmd_UpdateRate ;
    ROS_INFO(ss1.str().c_str());

// COM port connection code
//    SerialStream serial_port_left;
//    SerialStream serial_port_right;
//Please not that you will have to change this value as per as the address of the Digital Compass in your robot
//    serial_port_left.Open( port_left.c_str() ) ;
//    serial_port_right.Open( port_right.c_str() ) ;
//    serial_port_left.Open( "/dev/ttyUSB0" ) ;
//    serial_port_right.Open( "/dev/ttyUSB1" ) ;

//  if( CheckSerialPort()) ROS_INFO("Start serial_port_left");
//  else ROS_ERROR("Failed to start serial_port_left");

//int CheckSerialPort(serial_port_left,port_left,int serial_port_baud,int stop_bit)
 
  if( CheckSerialPort( &serial_port_left , &port_left , port_baud , 2) ) ROS_INFO("Start serial_port_left");
  else ROS_ERROR("Failed to start serial_port_left");
  if( CheckSerialPort( &serial_port_right, &port_right , port_baud , 2) ) ROS_INFO("Start serial_port_right");
  else ROS_ERROR("Failed to start serial_port_right");

 if ((serial_port_left.good() & serial_port_right.good()) != 0 )  ROS_INFO("Both Serial Port are Good! ");
  else {
	ROS_ERROR("Fail at least one of the serial port can't open ");
	serial_port_right.Close();
	serial_port_left.Close();
	ROS_ERROR("Quit qsDrivetrain 1");
        exit(1);
	return 0;
	};

// if serial [port can't open] leave
//  return 0;



















  /**
   * The ros::init() function needs to see argc and argv so that it can 
   * perform any ROS arguments and name remapping that were provided at
   * the command line. For programmatic remappings you can use a
   * different version of init() which takes remappings directly, but
   * for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any
   * other part of the ROS system.
   */


  /**
   * NodeHandle is the main access point to communications with the
   * ROS system. The first NodeHandle constructed will fully initialize
   * this node, and the last NodeHandle destructed will close down
   * the node.
   */
// %Tag(NODEHANDLE)%
// %EndTag(NODEHANDLE)%

std::string tf_prefix_ = tf::getPrefixParam(node_);

// Run spin() in different thread
   boost::thread spin_thread = boost::thread(boost::bind(&spinThread));

  /**
   * The advertise() function is how you tell ROS that you want to
   * publish on a given topic name. This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing. After this advertise() call is made, the master
   * node will notify anyone who is trying to subscribe to this topic name,
   * and they will in turn negotiate a peer-to-peer connection with this
   * node.  advertise() returns a Publisher object which allows you to
   * publish messages on that topic through a call to publish().  Once
   * all copies of the returned Publisher object are destroyed, the topic
   * will be automatically unadvertised.
   *
   * The second parameter to advertise() is the size of the message queue
   * used for publishing messages.  If messages are published more quickly
   * than we can send them, the number here specifies how many messages to
   * buffer up before throwing some away.
   */
// %Tag(PUBLISHER)%
ros::Publisher chatter_pub = node_.advertise<std_msgs::String>("chatter", 1000);
ros::Publisher odom_pub_   = node_.advertise<nav_msgs::Odometry>("odom", 1);
// %EndTag(PUBLISHER)%

// %Tag(Subscriber)%
ros::Subscriber sub          = node_.subscribe("chatter", 1000, chatterCallback);
ros::Subscriber cmd_vel_sub_ = node_.subscribe<geometry_msgs::Twist>("cmd_vel", 1, cmdVelReceived);
// %EndTag(Subscriber)%

// %Tag(LOOP_RATE)%
  ros::Rate loop_rate(cmd_UpdateRate);
// %EndTag(LOOP_RATE)%

  /**
   * A count of how many messages we have sent. This is used to create
   * a unique string for each message.
   */




// TODO add qsController initilization code here


    SetSpeed(0, 0);
    GetTics(ltics_last, rtics_last, tics_time_last);

  ROS_INFO("qsDrivetrain driver ready");








// %Tag(ROS_OK)%
  int count = 0;
  while (ros::ok() )
  {
// %EndTag(ROS_OK)%
    /**
     * This is a message object. You stuff it with data, and then publish it.
     */
// %Tag(FILL_MESSAGE)%
    std_msgs::String msg;

    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();
// %EndTag(FILL_MESSAGE)%

// %Tag(ROSCONSOLE)%
    ROS_INFO("%s", msg.data.c_str());
// %EndTag(ROSCONSOLE)%

    /**
     * The publish() function is how you send messages. The parameter
     * is the message object. The type of this object must agree with the type
     * given as a template parameter to the advertise<>() call, as was done
     * in the constructor above.
     */

//=send data to serial=======================================================
/*
char data_to_send ='$';
serial_port_left.write( &data_to_send, 1 ) ;
serial_port_left << 'U' ;
// You can write a whole string. 
serial_port_left << "Hello, Serial Port." << std::endl ;
double radius = 2.0 ;
double area = M_PI * 2.0 * 2.0 ;
serial_port_left << "radius=" << radius << "; area="<< area << std::endl ;
*/
serial_port_left << "Hello, Serial Port Left." << count << std::endl ;
serial_port_right << "Hello, Serial PortRight."<< count << std::endl ;
  ROS_INFO("Serail Data send");

//=get data from serial=======================================================
    //while( (serial_port_left.rdbuf()->in_avail() == 0 )&&(ros::ok()))  { usleep(100) ; } // this will make program stuck
/*
    // Keep reading data from serial port and print it to the screen.
    while( serial_port_left.rdbuf()->in_avail() > 0  ) 
    {
        char next_byte;
        serial_port_left.get(next_byte);
//        std::cerr << std::hex << static_cast<int>(next_byte) << " ";
        std::cerr << next_byte ;
        usleep(100) ;
    } 
    std::cerr << std::endl ;*/
// Read a whole array of data from the serial port. 
//
// @115200BPS , each character is 12bit time ( 1 start , 8 data , 2 stop bit + 1bit off )
// each byte takes 11/115200 seconds to send , if send 30 bytes, needs 2.86ms to send
 usleep(5000);
/*
const int BUFFER_SIZE = 256 ;
char input_buffer[BUFFER_SIZE] ; 
//
if ( serial_port_left.rdbuf()->in_avail() > 0  ) 
	{
	int data_read_size=serial_port_left.rdbuf()->in_avail();

	serial_port_left.read( input_buffer,  data_read_size ) ;
	input_buffer[data_read_size]=0x00;
	 ROS_INFO("Serail Data read Left:");
	    int i=0;
	    while( input_buffer[i] != 0  ) std::cerr << input_buffer[i++] ;
	    std::cerr << std::endl ;

	}
if ( serial_port_right.rdbuf()->in_avail() > 0  ) 
	{
	int data_read_size=serial_port_right.rdbuf()->in_avail();

	serial_port_right.read( input_buffer,  data_read_size ) ;
	input_buffer[data_read_size]=0x00;
	  ROS_INFO("Serail Data read Right:");
	    int i=0;
	    while( input_buffer[i] != 0  ) std::cerr << input_buffer[i++] ; 
	    std::cerr << std::endl ;

	}
//  ROS_INFO("Serail Data read");
*/

// TODO update Odom
UpdateOdom();

// TODO send speed command if we have new cmd_vel in, also check the watchdog
// if timeout , send speed 0 to motor.
	if (new_command_flag==1)
	{
	 SetSpeed(cmd_vel_vx,cmd_vel_va);
	 new_command_flag=0;

	}
	if((last_command_time-ros::Time::now().toSec())>watchdog_software_time)
{
 SetSpeed(0,0);
}

       time_cur = ros::Time::now().toSec();
        /* Hardware Watchdog */
        /* This is to pet the hardware watchdog */
        if (watchdog_hardware_time != 0) {
            if (time_cur - time_last_watchdog >= watchdog_hardware_time)
                /* Obviously this time should be set to less than the actual
                   hardware time */
            {
                PetWatchDog();
                time_last_watchdog = time_cur;
            }
        }







/*
ROS_INFO("Serail Data Left read:");
    while( serial_port_left.rdbuf()->in_avail() > 0  ) 
    {
        char next_byte;
        serial_port_left.get(next_byte);
        std::cerr << next_byte ;
    } 
    std::cerr << std::endl ;
ROS_INFO("Serail Data Right read:");
    while( serial_port_right.rdbuf()->in_avail() > 0  ) 
    {
        char next_byte;
        serial_port_right.get(next_byte);
        std::cerr << next_byte ;
    } 
    std::cerr << std::endl ;*/








//=update odom=======================================================
            nav_msgs::Odometry odom;

            // Translate from Player data to ROS data
            odom.pose.pose.position.x = POS_x;
            odom.pose.pose.position.y = POS_y;
            odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(POS_theta);

            odom.twist.twist.linear.x = POS_vx;
            odom.twist.twist.linear.y = POS_vy;
            odom.twist.twist.angular.z =POS_va;

            //@todo TODO: Think about publishing stall information with odometry or on a separate topic
            //odom.stall = pdata->stall;

            odom.header.frame_id = tf::resolve(tf_prefix_, odom_frame_id);

	    odom.header.stamp = ros::Time::now();

            ///odom.header.stamp.sec = (long long unsigned int) floor(hdr->timestamp);
            ///odom.header.stamp.nsec = (long long unsigned int) ((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL);

            populateCovariance(odom);

            // Publish the new data
            odom_pub_.publish(odom);

//=end of update odom=======================================================


// %Tag(PUBLISH)%
    chatter_pub.publish(msg);
// %EndTag(PUBLISH)%

// %Tag(SPINONCE)%
//    ros::spinOnce(); // move to seprate thread
// %EndTag(SPINONCE)%

// %Tag(RATE_SLEEP)%
    loop_rate.sleep();
// %EndTag(RATE_SLEEP)%
    ++count;





 if ((serial_port_left.good() & serial_port_right.good()) != 0 )  ROS_INFO("Both Serial Port are Good! ");
  else {
	ROS_ERROR("Fail at least one of the serial port has Fault! Maybe cable unpluged! ");
	serial_port_right.Close();
	serial_port_left.Close();
	ROS_ERROR("Quit qsDrivetrain at serial port fault! ");
        exit(2);
	return 0;
	};


  }
// Do pre-shutdown tasks
 ros::shutdown();

// stop the thread for ros::spin()

spin_thread.join();
serial_port_left.Close();
serial_port_left.Close();
printf("Shutdown qsDrivetrain ! \n");

  return 0;
}



/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
    void populateCovariance(nav_msgs::Odometry &msg)
    {
        if (fabs(msg.twist.twist.linear.x) <= 1e-8 &&
            fabs(msg.twist.twist.linear.y) <= 1e-8 &&
            fabs(msg.twist.twist.linear.z) <= 1e-8)
        {
            //nav_msgs::Odometry has a 6x6 covariance matrix
            msg.pose.covariance[0] = 1e-12;
            msg.pose.covariance[7] = 1e-12;
            msg.pose.covariance[35] = 1e-12;
        }
        else
        {
            // nav_msgs::Odometry has a 6x6 covariance matrix
            msg.pose.covariance[0] = pow(sigma_x_, 2);
            msg.pose.covariance[7] = pow(sigma_y_, 2);
            msg.pose.covariance[35] = pow(sigma_theta_, 2);
        }
        
        msg.pose.covariance[14] = DBL_MAX;
        msg.pose.covariance[21] = DBL_MAX;
        msg.pose.covariance[28] = DBL_MAX;
        
        msg.twist.covariance = msg.pose.covariance;
    }

    void cmdVelReceived(const geometry_msgs::Twist::ConstPtr& cmd_vel)
    {
        last_command_time  =   ros::Time::now().toSec();
	new_command_flag=1;
        cmd_vel_vx = cmd_vel->linear.x;
        cmd_vel_vy = 0.0;
        cmd_vel_va = cmd_vel->angular.z;
        cmd_state = 1;
    std::stringstream ss;
    ss << "Got cmd_vel vx: " << cmd_vel_vx << "; va: " << cmd_vel_va;
    ROS_INFO(ss.str().c_str());
    }

void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}
// Use different Thread to spin(); (get message) forever
void spinThread()
{
    ros::spin();
}








// modify from libserial example
int CheckSerialPort(SerialStream* serial_port,std::string* port,int serial_port_baud,int stop_bit)
{
//    SerialStream serial_port;

    (*serial_port).Open( *port ) ;

    //Error management for serial port
    if ( ! (*serial_port).good() ) 
    {
        std::cerr << "[" << __FILE__ << ":" << __LINE__ << "] "
                  << "Error: Could not open serial port:" << *port
                  << std::endl ;
       return 0 ;
    }
    //
    // Set the baud rate of the serial port.
    //


   switch (serial_port_baud) {
        case 9600:
            (*serial_port).SetBaudRate( SerialStreamBuf::BAUD_9600 ) ;
            break;
        case 38400:
            (*serial_port).SetBaudRate( SerialStreamBuf::BAUD_38400 ) ;
            break;
        case 57600:
            (*serial_port).SetBaudRate( SerialStreamBuf::BAUD_57600 ) ;
            break;
        case 115200:
            if(SerialStreamBuf::BAUD_115200==SerialStreamBuf::BAUD_INVALID) std::cerr << "Error: Please update your libserial0 to 0.61, http://code.google.com/p/amrl-ros-pkg/downloads/detail?name=libserial-0.6.1.tar.gz" << std::endl ;

            (*serial_port).SetBaudRate( SerialStreamBuf::BAUD_115200 ) ;
            break;
        case 230400:
            (*serial_port).SetBaudRate( SerialStreamBuf::BAUD_230400 ) ;
            break;
        default:
            printf("Unknown baud rate [%d] defaulting to 115200\n",serial_port_baud);
            (*serial_port).SetBaudRate( SerialStreamBuf::BAUD_115200 ) ;
            break;
    }

    //(*serial_port).SetBaudRate( SerialStreamBuf::BAUD_115200 ) ;
    if ( ! (*serial_port).good() ) 
    {
        std::cerr << "Error: Could not set the baud rate." << std::endl ;
          return 0 ;
    }
    //
    // Set the number of data bits.
    //
    (*serial_port).SetCharSize( SerialStreamBuf::CHAR_SIZE_8 ) ;
    if ( ! (*serial_port).good() ) 
    {
        std::cerr << "Error: Could not set the character size." << std::endl ;
         return 0 ;
    }
    //
    // Disable parity.
    //
    (*serial_port).SetParity( SerialStreamBuf::PARITY_NONE ) ;
    if ( ! (*serial_port).good() ) 
    {
        std::cerr << "Error: Could not disable the parity." << std::endl ;
          return 0 ;
    }
    //
    // Set the number of stop bits to 2 for QuickSilver Motor Controller
    //

  switch (stop_bit) {
        case 1:
            (*serial_port).SetNumOfStopBits( 1 )  ;
            break;
        case 2:
            (*serial_port).SetNumOfStopBits( 2 )  ;
            break;
        default:
            printf("Unknown Stop bits [%d] defaulting to 2\n",stop_bit);
           (*serial_port).SetNumOfStopBits( 2 )  ;
            break;
    }
    //(*serial_port).SetNumOfStopBits( 2 ) ;
    if ( ! (*serial_port).good() ) 
    {
        std::cerr << "Error: Could not set the number of stop bits."
                  << std::endl ;
         return 0 ;
    }
    //
    // Turn off hardware flow control.
    //
    (*serial_port).SetFlowControl( SerialStreamBuf::FLOW_CONTROL_NONE ) ;
    if ( ! (*serial_port).good() ) 
    {
        std::cerr << "Error: Could not use hardware flow control."
                  << std::endl ;
          return 0 ;
    }
 return 1 ;

}




inline int32_t
 MPSToSVU(double mps, double wc) {
    /* mps/wc = RPS */
    /* From SilverLode User Guide: 2^31 SVU/4000 RPM * 60RPM/RPS
           = 32,212,254.705 SVU/RPS */
#define POWER_2_31 2147483648.0 /* (double) 2 ^ 31 */

    double svu;
    svu = (POWER_2_31 * mps) / ( motor_max_rpm * wc ) *
           60.0 * motor_gearbox_ratio;

    return (int32_t) rint(svu);
}

inline double
 TicsToMeters(int tics, double wc, double tpr)
/* Encoder Tics, Wheel Circumference, Tics Per Rev */
{
    double meters;
    meters = (tics * wc)/tpr;
    return meters;
}

/* Updates posdata.px, posdata.py, posdata.pa and posdata.vx, posdata.va */
int
 UpdateOdom(double pyaw) {
    int32_t ltics, rtics;
    int ltics_delta, rtics_delta;
    double Lmeters, Rmeters;
    double tics_time_delta, tics_time;
    double trans_delta, angle_delta;

    /* Get the current tics from the encoder */
    if (GetTics(ltics, rtics, tics_time)) {
        /* GetTics failed */
        /* Wait and try again */
        usleep(5000);
        if (GetTics(ltics, rtics, tics_time)) {
            /* Failed again. We're screwed. */
            return -1;
		//exit(-1);
        }
    }
    LeftTics=((int) ltics);
    RightTics=( (int) rtics);


    /* Find the delta and save the current tics (for next cycle) */
    tics_time_delta = tics_time - tics_time_last;
    ltics_delta = ltics - ltics_last;
    rtics_delta = rtics - rtics_last;

    tics_time_last = tics_time;
    ltics_last = ltics;
    rtics_last = rtics;

    /* Convert to meters */
    Lmeters = TicsToMeters(ltics_delta, wheelcircumference,
                           tics_per_rev * motor_gearbox_ratio);
    Rmeters = TicsToMeters(rtics_delta, wheelcircumference,
                           tics_per_rev * motor_gearbox_ratio);

    /* Convert to vehicle values, not wheel values */
    trans_delta = (Lmeters + Rmeters)/2;
    /* Translational distance (in x/forward direction) traveled this cycle */
    if(isnan(pyaw))
        angle_delta = (Rmeters - Lmeters)/wheelbase;
    else
        angle_delta = wrapToPI(pyaw - POS_theta);
    /* Angular change (in yaw) this cycle */

    /* Compute Velocities */
    POS_vx = trans_delta / (tics_time_delta/1000); // convert ms to sec
    /* Linear velocity */

    POS_va = angle_delta / (tics_time_delta/1000); 
    /* Angular velocity */

    /* Add to our current pose */
    POS_x += trans_delta * cos(POS_theta + angle_delta/2);
    POS_y += trans_delta * sin(POS_theta + angle_delta/2);

    if(!isnan(pyaw))
        POS_theta = pyaw;
    else
        POS_theta = wrapToPI(POS_theta + angle_delta);

    return 0;
}

int
 GetTics(int32_t &tics_left, int32_t &tics_right, double &tics_time) {
    char cmd_left[25], cmd_right[25];
    char buff_left[25], buff_right[25];
    int len_left, len_right;
    int status;


    Port_flush(&serial_port_left);
    Port_flush(&serial_port_right);

    len_left = snprintf(cmd_left, 25, "@%d %d %d\r", 
                        left_id, RRG, QS_REG_ENCODER);
    len_right = snprintf(cmd_right, 25, "@%d %d %d\r", 
                        right_id, RRG, QS_REG_ENCODER);

    //tics_time = GetTime();
    tics_time =ros::Time::now().toSec();

    status = qsQuery(cmd_left, len_left, buff_left,
                     cmd_right, len_right, buff_right, 25);
    if (status == 0) { /* No errors */
        uint16_t tics_low;
        uint16_t tics_high;

        /* Line should read as: # 01 000C 0000 0001 */
        /* All numbers are hex:
           # (Ack)
           01 (ID Number of motor)
           000C (Register that was read)
           0000
           0001 (The value from the register)
        */

        /* The 10 comes from strlen("# 01 000C ") */
        sscanf(buff_left + 10, "%4x %4x",
               (unsigned int *) &tics_high, (unsigned int *) &tics_low);
        tics_left = (tics_high << 16) | tics_low;

        sscanf(buff_right + 10, "%4x %4x",
               (unsigned int *) &tics_high, (unsigned int *) &tics_low);
        tics_right = (tics_high << 16) | tics_low;
    } else {
        /* We got an error getting encoder data. This is bad. */
        ROS_ERROR("qsDrivetrain: Error getting encoder data.");
        tics_left = 0;
        tics_right = 0;
        tics_time = 0;
    }
    return status;

}


/* Function allowing querying of two motors at virtually the same time assuming
 * two serial ports are used. If only a single port is used, 
 * the code still works, just with added latency
 */
int qsQuery(char *cmd_left, int cmd_len_left, char * output_left,
                      char *cmd_right, int cmd_len_right, char * output_right,
                      int  maxlen)
{
    int output_len_left, output_len_right;
    struct aiocb *aiocbps[2];
    int8_t left_status = 0;
    int8_t right_status = 0;
    /* Basic idea is if we have two serial ports:
       Async write command 1
       Async write command 2
       Wait for 1 to finish and read response.
       Wait for 2 to finish and read response.

       This way things like speed changes and encoder reads happen at
       Virtually the same time, rather than the left always responding quicker.
       
       However for tristate mode no such sequence is possible so we do:
       Async write command 1
       Wait for 1 to finish and read response.
       Async write command 2
       Wait for 2 to finish and read response.
       Async isn't needed for this case, but the code is built for two port mode,
       With single port mode only supported because at the time of writing I don't
       have two port hardware ready.
    */

///    if (cmd_len_left > 0) {
///        aiocbps[0] = AioWriteBuf(this->fd_left, cmd_left, cmd_len_left);
///        if (this->serial_single_port) {
///            /* Single port mode. Read left respones before writing right command */
///            aio_suspend(aiocbps, 1, 0); /* Wait for write to finish */
///            aio_return(aiocbps[0]);
///            output_len_left = ReadLine(this->fd_left, output_left, maxlen, READ_TIMEOUT);
///            /* Then attempt to read */
///        }
///    }
	if (cmd_len_left  > 0) serial_port_left.write( cmd_left , cmd_len_left);
        if (cmd_len_right > 0) serial_port_right.write(cmd_right, cmd_len_right);

///    if (! this->serial_single_port && cmd_len_left > 0) {
///        /* Haven't read from left yet, and we sent something to left */
///        aio_suspend(aiocbps, 1, 0); /* Wait for write to finish */
///        aio_return(aiocbps[0]);
///        output_len_left = ReadLine(this->fd_left, output_left, maxlen, READ_TIMEOUT);
///        /* Then attempt to read */
///    }


	output_len_left  =ReadLine(&serial_port_left,output_left, 25, 500 );
	output_len_right =ReadLine(&serial_port_right,output_left, 25, 500 );

        /*std::string S_output_left= serial_port_left.ReadLine( READ_TIMEOUT,'\n');
	std::string S_output_right= serial_port_right.ReadLine(READ_TIMEOUT,'\n');
        S_output_left.copy(output_left,S_output_left.length,0);
        S_output_right.copy(output_right,S_output_right.length,0);*/
	//output_left << serial_port_left.ReadLine( READ_TIMEOUT,'\n');
        //output_right << serial_port_right.ReadLine(READ_TIMEOUT,'\n');
///    if (cmd_len_right > 0) {
///        /* Sent something to right, so read it */
///        aio_suspend(aiocbps+1, 1, 0); /* Wait for write to finish */
///        aio_return(aiocbps[1]);
///        output_len_right = ReadLine(this->fd_right, output_right, maxlen, READ_TIMEOUT);
///        /* Then attempt to read */
///    }

    /* Error checking */
    left_status =  CheckResponseCode(output_len_left,  output_left);
    right_status = CheckResponseCode(output_len_right, output_right);

    return left_status + (right_status << 4);

}

int ReadLine(SerialStream *serial_port,char * output_buff, int  maxlen, int timout ){
	int i=0;
	do{
        // TODO need a timeout check here
        
          char next_byte;
          (*serial_port).get(next_byte);
	*output_buff=next_byte;
	if ((next_byte)=='\n') break;
	output_buff++;
	i++;
        maxlen--;
	} while(maxlen>0);

	return i;

	}


/*
	{
	std::stringstream ss;
        
        do{
          char next_byte;
          serial_port.get(next_byte);
          ss<<(next_byte);
        //std::cerr << std::dec << static_cast<int>(next_byte) << " \n";
                //std::cout << (next_byte) << "";
        //printf(" one byte received... \n");
          usleep(1) ;
       // std::cerr<<"yay I read something"; 
          if ((next_byte)=='\n') break;
           
          }while (1);

         std::cout<<ss.str() <<"";

	}*/

inline int
 CheckResponseCode(int len, char *buff) {
    int status = 0;
    if (len > 0) { /* Got a response */
        switch ( buff[0] ) {
            case '#':
            case '*':
                status = 0;
                /* Good ! */
                break;
            case '!':
            default:
                status = 1;
                /* Crap */
                break;
        }
    } else {
        status = 1;
    }
    return status;
}

/* SetSpeed( Linear Meters Per Second, Rotational Radians Per Second )
   Returns 1 on success, 0 otherwise
*/
int
 SetSpeed(double trans_vel, double yaw_rad_per_sec) {
    char cmd_left[25], cmd_right[25];
    char buff_left[25], buff_right[25];
    int len_left, len_right;

    double vel_left, vel_right;

    double bias = yaw_rad_per_sec * wheelbase;
    /* Sanity check on data. Make sure we're not too fast */
    if (trans_vel > max_trans_vel)
        trans_vel = max_trans_vel;
    else if (trans_vel < -max_trans_vel)
        trans_vel = -max_trans_vel;

    if (bias > max_bias_vel)
        bias = max_bias_vel;
    else if (bias < -max_bias_vel)
        bias = -max_bias_vel;

    /* After sanity check, calculate the individual motor velocities */
    vel_left = trans_vel - bias/2;
    vel_right = trans_vel + bias/2;

    /* Convert into "counts" to send to the motor */
    int32_t svu_left, svu_right;
    /* The motors don't just use tics per second, they have a scale factor too,
       That's why this is called Counts not tics */
    svu_left = MPSToSVU(vel_left, wheelcircumference);
    svu_right = MPSToSVU(vel_right, wheelcircumference);


    /* Generate the command and send it */
    len_left = snprintf(cmd_left, 25, "@%d %d %d %d\r",
                       left_id, WRI,
                        QS_REG_PC_MODE_VELOCITY, svu_left);

    len_right = snprintf(cmd_right, 25, "@%d %d %d %d\r",
                         right_id, WRI,
                         QS_REG_PC_MODE_VELOCITY, svu_right);

    int status = qsQuery(cmd_left, len_left, buff_left,
                         cmd_right, len_right, buff_right, 25);

    //posdata.stall = 0;
    //printf("Got %d and [%s] and [%s]\n", status, buff_left, buff_right);
    return status;
}

int
 PetWatchDog() {
    char cmd_left[25], cmd_right[25];
    char buff_left[25], buff_right[25];
    int len_left, len_right;


    len_left  = snprintf(cmd_left, 25, "@%d %d %d %d\r", left_id, WRI,
                         QS_REG_WATCHDOG, 0);
    len_right = snprintf(cmd_right, 25, "@%d %d %d %d\r", right_id, WRI,
                         QS_REG_WATCHDOG, 0);

    return  qsQuery(cmd_left, len_left, buff_left,
                    cmd_right, len_right, buff_right,
                    25);
}


inline double wrapTo2PI(double theta)
	{
    //Normalize an angle in radians to [0, 2*pi]
	double a=fmod(theta , 2*M_PI); // a is [-2*PI~2*PI]
	if (a<0) return (a+2*M_PI);    // if a<0 , change range to [0 ~ 2*PI] 
	else return a;
}

inline double wrapToPI(double theta)
	{
    //Normalize an angle in radians to [-pi, pi]
    return (wrapTo2PI(theta+M_PI) - M_PI); }


inline void Port_flush(SerialStream* serial_port )
{
	if ((*serial_port).rdbuf()->in_avail() >0){
	const int BUFFER_SIZE = (*serial_port).rdbuf()->in_avail();
	char input_buffer[BUFFER_SIZE] ;
	(*serial_port).read(input_buffer, BUFFER_SIZE);
	}
}







// %EndTag(FULLTEXT)%
